#include "ros/ros.h"
#include "fanguangbantrilateration/trilateration_read_time_log.h"
#include <locale>
#include <codecvt>
#include <string>
#include <sstream>
#include <map>
#include <geometry_msgs/Pose.h>

#include <iostream>
#include <vector>
/**
 *
 * 接收性能评估数据，控制台输出数据，中文数据。
 * 苏凯
 * 2025-2-8
 * rosrun   fanguangbantrilateration  fanguangbantrilateration_read_time_log_node
 * @param unicode_string
 * @return
 */
// 定义颜色的转义序列 std::cerr
const std::string RED = "\033[31m";
const std::string GREEN = "\033[32m";
const std::string YELLOW = "\033[33m";
const std::string BLUE = "\033[34m";
const std::string MAGENTA = "\033[35m";
const std::string CYAN = "\033[36m";
const std::string RESET = "\033[0m";


// 分割字符串函数
std::vector<std::string> splitString(const std::string& str, char delimiter) {
    std::vector<std::string> parts;
    size_t start = 0;
    size_t end = str.find(delimiter);

    while (end != std::string::npos) {
        // 提取子字符串并去除两端空格
        std::string part = str.substr(start, end - start);
        part.erase(0, part.find_first_not_of(" \n\r\t")); // 去除前导空格
        part.erase(part.find_last_not_of(" \n\r\t") + 1); // 去除尾部空格

        parts.push_back(part);

        // 更新索引
        start = end + 1;
        end = str.find(delimiter, start);
    }

    // 处理最后一个子字符串
    std::string lastPart = str.substr(start);
    lastPart.erase(0, lastPart.find_first_not_of(" \n\r\t")); // 去除前导空格
    lastPart.erase(lastPart.find_last_not_of(" \n\r\t") + 1); // 去除尾部空格
    parts.push_back(lastPart);

    return parts;
}

// 将 Unicode 转义字符串解码为 UTF-8 编码的中文字符串
std::string decodeUnicodeString(const std::string& unicode_string)
{
    std::string decoded_string;
    size_t pos = 0;
    size_t last_pos = 0;

    while ((pos = unicode_string.find("\\u", last_pos)) != std::string::npos) {
        decoded_string.append(unicode_string, last_pos, pos - last_pos);
        std::string hex_code = unicode_string.substr(pos + 2, 4);
        unsigned int code_point;
        std::stringstream ss;
        ss << std::hex << hex_code;
        ss >> code_point;
        char utf8_char[5] = {0};
        if (code_point <= 0x7F) {
            utf8_char[0] = code_point;
        } else if (code_point <= 0x7FF) {
            utf8_char[0] = 0xC0 | (code_point >> 6);
            utf8_char[1] = 0x80 | (code_point & 0x3F);
        } else if (code_point <= 0xFFFF) {
            utf8_char[0] = 0xE0 | (code_point >> 12);
            utf8_char[1] = 0x80 | ((code_point >> 6) & 0x3F);
            utf8_char[2] = 0x80 | (code_point & 0x3F);
        } else {
            utf8_char[0] = 0xF0 | (code_point >> 18);
            utf8_char[1] = 0x80 | ((code_point >> 12) & 0x3F);
            utf8_char[2] = 0x80 | ((code_point >> 6) & 0x3F);
            utf8_char[3] = 0x80 | (code_point & 0x3F);
        }
        decoded_string += std::string(utf8_char);
        last_pos = pos + 6;
    }

    decoded_string.append(unicode_string, last_pos);
    return decoded_string;
}
// 获取time_log_fig的描述信息
std::string getTimeLogFigDescription(int time_log_fig)
{
    std::map<int, std::string> timeLogFigDescriptions = {
            {-1, "全部关闭"},
            {0, "全部打开"},
            {-2, "只打开 1：FindCircle拟定圆心坐标，6：匹配算法，9：位姿推算"},
            {1, "FindCircle: 通过雷达强度数据，使用三角函数拟定圆心坐标，并存入反光板数据缓存中"},
            {2, "匹配数据: 通过得到的圆心坐标，在已经匹配成功的反光板数据缓存中匹配数据"},
            {3, "匹配数据: 通过得到的圆心坐标，在储存所有的反光板数据缓存中匹配数据"},
            {4, "匹配数据: 通过得到的圆心坐标，在反光板数据缓存中匹配数据"},
            {5, "匹配数据: 匹配走广度优先算法 或 匹配走广度优先算法 + 优先使用匹配好的反光板数据"},
            {6, "匹配算法：从查找圆心到匹配完成需要的总执行时间"},
            {7, "位姿推算: 基于之前 （ 已经存在 || 不存 ） 在匹配上的反光柱数据及当前匹配的反光柱数量小于3个时的移动机器人位置坐标计算"},
            {8, "位姿推算: 经典三边定位最小二乘法，推算位姿"},
            {9, "位姿推算: 基于之前已经匹配上了反光板 + 经典三边定位最小二乘法，推算位姿"},
            {10, "位姿推算: 所有使用默认的 tf_last_odom_to_map_transform_ 数据 发布的移动机器人坐标"},
            {11, "匹配算法: 反光柱匹配算法：只用特征匹配算法  需要匹配的边长数量"},
            {12, "位姿推算: 上一帧的位姿odomTbaselink与当前帧的位姿odomTbaselink之间的差异"},
            {13, "位姿推算: 未计算出有效的定位数据导致超时"},
            {14, "位姿推算: 未计算出有效的定位数据导致超时,通过检查不同条件来决定是否需要重新开始定位过程 [超时重新计算 (trilateration_s>targetTrilaterationTimeOut_)]"},
            {15, "位姿推算: 未计算出有效的定位数据导致超时,通过检查不同条件来决定是否需要重新开始定位过程 [判断当前机器人的定位数据是否不满足条件，移动机器人不在地图的自由区域中(result_Mapvalue_!=free_cell_)，不满足则重新计算"},
            {16, "位姿推算: 未计算出有效的定位数据导致超时,通过检查不同条件来决定是否需要重新开始定位过程 [超时重新计算,雷达与地图轮廓的结合率 bool is_ratio = (ratio >= matchingcostma_threshold_)，不满足则重新计算]"},
            {17, "位姿推算: 未计算出有效的定位数据导致超时,通过检查不同条件来决定是否需要重新开始定位过程 [超时重新计算,进入这个函数 evaluateTrilaterationTimeout，分别判断 14，15，16]"},
            {18, "匹配算法: 不满足条件移动机器人不在地图的自由区域中或者雷达与地图轮廓的结合率不满足阀值 【( max_ratio<matchingcostma_threshold_||   result_Mapvalue!=free_cell_)】"},
            {19, "匹配算法: 满足条件移动机器人在地图的自由区域中或者雷达与地图轮廓的结合率满足阀值"},
            {21, "心跳机制: 心跳机制，检查校验m atch_costmap_with_scan_node 节点是否启动 [ match_costmap_with_scan_node 节点工作异常或未启动，这可能会导致出现定位丢失问题]"},
            {22, "发布定位数据: 这里判断计算出来的定位与当前tf的定位误差值，小于 某个容忍阀值 就不更新定位，减少定位波动（波动会影响机器人运动的卡顿情况）。需要大于一定阀值时才会使用新的定位数据"},//time_log_fig_==22  是否原地旋转
            {23, "计算大致的机器人位姿：ComputeCurrentPose函数中匹配上的当前反光柱与全局反光柱的集合，返回出一个大致的机器人位姿。"},
    };

    auto it = timeLogFigDescriptions.find(time_log_fig);
    if (it != timeLogFigDescriptions.end()) {
        return it->second;
    } else {
        return "未知 time_log_fig";
    }
}

// 订阅的回调函数
void chatterCallback(const fanguangbantrilateration::trilateration_read_time_log::ConstPtr& msg)
{
    // 将 Unicode 转义字符串解码为中文字符串
    std::string decoded_chinese_message = decodeUnicodeString(msg->result_chinese_message);

    if(msg->time_log_fig==17||msg->time_log_fig==18||msg->time_log_fig==19){
        // 输出所有字段的数据
        std::cout <<GREEN<< "start========================================= time_log ==============================================================================" <<RESET<<  std::endl;
       // ROS_INFO("start========================================= time_log ==============================================================================");
        ROS_INFO("选择性能评估数据： Time Log Fig: %d - %s", msg->time_log_fig, getTimeLogFigDescription(msg->time_log_fig).c_str());
        ROS_INFO("Result Type: %s", msg->result_type.c_str());
        ROS_INFO("Class Name: %s", msg->className.c_str());
        ROS_INFO("Class Line: %d", msg->classLine);

        // 调用函数
        std::vector<std::string> parts = splitString(decoded_chinese_message, '|');


        int ii=0;
        // 输出结果
        for (const auto& part : parts) {
          if (ii == 0) {

              std::cout <<YELLOW<< part <<RESET<<  std::endl;
            } else if (ii == 1) {
                std::cout << CYAN << part << RESET << std::endl;
            }  else if (ii == 2){
              //  std::cout << "\033[31m" << part << "\033[0m" << std::endl; // 红色输出
                std::cout << BLUE << part << RESET << std::endl;
            } else {
              //  std::cout << "\033[31m" << part << "\033[0m" << std::endl; // 红色输出
              std::cout << GREEN << part << RESET << std::endl;
          }

            ii++;
        }



        ROS_INFO("Result Message: %s", msg->result_message.c_str());
        ROS_INFO("匹配上的反光柱数量： matchokReflectorPanelPointEnd_Unordered_map_size，num_points: %d", msg->num_points);
        ROS_INFO("匹配上的数据ids： matchReflectorPaneIds，num_points: %s", msg->matchReflectorPaneIds.c_str());
        // 使用ROS_INFO打印Pose对象
        ROS_INFO("计算得到的移动机器人位置 mapTbase_link-> Position: (%f, %f, %f)", msg->mapTbase_link.position.x, msg->mapTbase_link.position.y, msg->mapTbase_link.position.z);
        ROS_INFO("计算得到的移动机器人姿态四元素 mapTbase_link-> Orientation: (%f, %f, %f, %f)", msg->mapTbase_link.orientation.x, msg->mapTbase_link.orientation.y, msg->mapTbase_link.orientation.z, msg->mapTbase_link.orientation.w);
        ROS_INFO("算得到的移动机器人姿态 mapTbase_link_yaw: %f", msg->mapTbase_link_yaw);

        ROS_INFO("队列数量： matchokReflectorPanelPointSizes: %d",  msg->matchokReflectorPanelPointSize);
        //ROS_INFO("耗时： Execution Time: %s ms", msg->execution_time.c_str());
        std::cout <<GREEN << "耗时： Execution Time:: "<< msg->execution_time<<  RESET<< std::endl;

        ROS_INFO("end=======================================================================================================================");
    }else if(msg->time_log_fig == 12){
        // 输出所有字段的数据
        ROS_INFO("start========================================= time_log ==============================================================================");
        ROS_INFO("选择性能评估数据： Time Log Fig: %d - %s", msg->time_log_fig, getTimeLogFigDescription(msg->time_log_fig).c_str());
        ROS_INFO("Result Type: %s", msg->result_type.c_str());
        ROS_INFO("Class Name: %s", msg->className.c_str());
        ROS_INFO("Class Line: %d", msg->classLine);
        //ROS_INFO("Decoded Chinese Message: %s", decoded_chinese_message.c_str());
        std::cout << YELLOW<< "Decoded Chinese Message: "<< decoded_chinese_message <<  RESET<< std::endl;
        ROS_INFO("English Message: %s", msg->result_EN_message.c_str());

        ROS_INFO("Result Message: %s", msg->result_message.c_str());
        ROS_INFO("---------------------------------------- domTbaselink --------------------------------------------------------");
        ROS_INFO("上一帧 previous_odomTbase_link 的位置  previous_odomTbase_link-> Position: (%f, %f, %f)", msg->previous_odomTbase_link.position.x, msg->previous_odomTbase_link.position.y, msg->previous_odomTbase_link.position.z);
        ROS_INFO("上一帧 previous_odomTbase_link 的姿态  previous_odomTbase_link previous_odomTbase_link-> Orientation: (%f, %f, %f, %f)", msg->previous_odomTbase_link.orientation.x, msg->previous_odomTbase_link.orientation.y, msg->previous_odomTbase_link.orientation.z, msg->previous_odomTbase_link.orientation.w);
        ROS_INFO("当前帧 predicted_odomTbase_link 的位置  predicted_odomTbase_link-> Position: (%f, %f, %f)", msg->predicted_odomTbase_link.position.x, msg->predicted_odomTbase_link.position.y, msg->predicted_odomTbase_link.position.z);
        ROS_INFO("当前帧 predicted_odomTbase_link 的姿态  predicted_odomTbase_link-> Orientation: (%f, %f, %f, %f)", msg->predicted_odomTbase_link.orientation.x, msg->predicted_odomTbase_link.orientation.y, msg->predicted_odomTbase_link.orientation.z, msg->predicted_odomTbase_link.orientation.w);
        ROS_INFO("上一帧 previous_odomTbase_link_yaw  自身角度 previous_odomTbase_link previous_odomTbase_link_yaw: (%f)", msg->previous_odomTbase_link_yaw);
        ROS_INFO("当前帧 predicted_odomTbase_link_yaw 自身角度 predicted_odomTbase_link predicted_odomTbase_link_yaw: (%f)", msg->predicted_odomTbase_link_yaw);
        //ROS_INFO("计算旋转角度差 diff_previous_with_predicted_odomTbase_link_yaw: %f", msg->diff_previous_with_predicted_odomTbase_link_yaw);
        std::cout <<GREEN << "计算旋转角度差: "<< std::to_string(msg->diff_previous_with_predicted_odomTbase_link_yaw)<<" ,角度阀值: "<< std::to_string(msg->previous_with_predicted_distance_angle_threshold)<<   RESET<< std::endl;
        //ROS_INFO("角度阀值 previous_with_predicted_distance_angle_threshold: %f", msg->previous_with_predicted_distance_angle_threshold);
        ROS_INFO("计算位移x diff_previous_with_predicted_odomTbase_link_x: %f", msg->diff_previous_with_predicted_odomTbase_link_x);
        ROS_INFO("计算位移y diff_previous_with_predicted_odomTbase_link_y: %f", msg->diff_previous_with_predicted_odomTbase_link_y);
        //ROS_INFO("距离差 欧式距离 previous_with_predicted_distance: %f", msg->previous_with_predicted_distance);
        std::cout <<GREEN << "距离差 欧式距离: "<< std::to_string(msg->previous_with_predicted_distance)<<" ,欧式距离阀值: "<< std::to_string(msg->previous_with_predicted_distance_threshold)<<  RESET<< std::endl;
        //ROS_INFO("欧式距离阀值  previous_with_predicted_distance_threshold: %f", msg->previous_with_predicted_distance_threshold);

        ROS_INFO("end=======================================================================================================================");
    }else if(msg->time_log_fig == 22){  //time_log_fig_==22  是否原地旋转
        // 输出所有字段的数据
        ROS_INFO("start========================================= time_log ==============================================================================");
        ROS_INFO("选择性能评估数据： Time Log Fig: %d - %s", msg->time_log_fig, getTimeLogFigDescription(msg->time_log_fig).c_str());
        ROS_INFO("Result Type: %s", msg->result_type.c_str());
        ROS_INFO("Class Name: %s", msg->className.c_str());
        ROS_INFO("Class Line: %d", msg->classLine);
        //ROS_INFO("Decoded Chinese Message: %s", decoded_chinese_message.c_str());

        ROS_INFO("English Message: %s", msg->result_EN_message.c_str());

        ROS_INFO("Result Message: %s", msg->result_message.c_str());
        ROS_INFO("---------------------------------------- mapTbaselink --------------------------------------------------------");
        ROS_INFO("反光柱计算得到 calculate_mapTbase_link 的位置  calculate_mapTbase_link-> Position: (%f, %f, %f)", msg->calculate_mapTbase_link.position.x, msg->calculate_mapTbase_link.position.y, msg->calculate_mapTbase_link.position.z);
        ROS_INFO("反光柱计算得到 calculate_mapTbase_link 的姿态   calculate_mapTbase_link-> Orientation: (%f, %f, %f, %f)", msg->calculate_mapTbase_link.orientation.x, msg->calculate_mapTbase_link.orientation.y, msg->calculate_mapTbase_link.orientation.z, msg->previous_odomTbase_link.orientation.w);
        ROS_INFO("反光柱计算得到 calculate_mapTbase_link_yaw  自身角度 calculate_mapTbase_link calculate_mapTbase_link: (%f)", msg->calculate_mapTbase_link_yaw);

        ROS_INFO("当前订阅tf得到 predicted_mapTbase_link 的位置  predicted_mapTbase_link-> Position: (%f, %f, %f)", msg->predicted_mapTbase_link.position.x, msg->predicted_mapTbase_link.position.y, msg->predicted_mapTbase_link.position.z);
        ROS_INFO("当前订阅tf得到 predicted_mapTbase_link 的姿态  predicted_mapTbase_link-> Orientation: (%f, %f, %f, %f)", msg->predicted_mapTbase_link.orientation.x, msg->predicted_mapTbase_link.orientation.y, msg->predicted_mapTbase_link.orientation.z, msg->predicted_mapTbase_link.orientation.w);
        ROS_INFO("当前订阅tf得到 predicted_mapTbase_link_yaw 自身角度 predicted_mapTbase_link predicted_mapTbase_link_yaw: (%f)", msg->predicted_mapTbase_link_yaw);
        //---------------------------------------------------------------------------------------------------------------------------------------
        ROS_INFO("---------------------------------------- domTbaselink --------------------------------------------------------");
        ROS_INFO("上一帧 previous_odomTbase_link 的位置  previous_odomTbase_link-> Position: (%f, %f, %f)", msg->previous_odomTbase_link.position.x, msg->previous_odomTbase_link.position.y, msg->previous_odomTbase_link.position.z);
        ROS_INFO("上一帧 previous_odomTbase_link 的姿态  previous_odomTbase_link previous_odomTbase_link-> Orientation: (%f, %f, %f, %f)", msg->previous_odomTbase_link.orientation.x, msg->previous_odomTbase_link.orientation.y, msg->previous_odomTbase_link.orientation.z, msg->previous_odomTbase_link.orientation.w);
        ROS_INFO("上一帧 previous_odomTbase_link_yaw  自身角度 previous_odomTbase_link previous_odomTbase_link_yaw: (%f)", msg->previous_odomTbase_link_yaw);
        //---------------------------------------------------------------------------------------------------------------------------------------
        ROS_INFO("当前帧 predicted_odomTbase_link 的位置  predicted_odomTbase_link-> Position: (%f, %f, %f)", msg->predicted_odomTbase_link.position.x, msg->predicted_odomTbase_link.position.y, msg->predicted_odomTbase_link.position.z);
        ROS_INFO("当前帧 predicted_odomTbase_link 的姿态  predicted_odomTbase_link-> Orientation: (%f, %f, %f, %f)", msg->predicted_odomTbase_link.orientation.x, msg->predicted_odomTbase_link.orientation.y, msg->predicted_odomTbase_link.orientation.z, msg->predicted_odomTbase_link.orientation.w);
        ROS_INFO("当前帧 predicted_odomTbase_link_yaw 自身角度 predicted_odomTbase_link predicted_odomTbase_link_yaw: (%f)", msg->predicted_odomTbase_link_yaw);
        //---------------------------------------------------------------------------------------------------------------------------------------
        std::cout << YELLOW<< "Decoded Chinese Message: "<< decoded_chinese_message <<  RESET<< std::endl;
        ROS_INFO("计算位置方差 diff_variancePose_min: %f", msg->diff_variancePose_min);
        ROS_INFO("计算角度方差 diff_varianceYaw_min: %f", msg->diff_varianceYaw_min);
        ROS_INFO("位置方差阀值 variance_min_pose_lerance: %f", msg->variance_min_pose_lerance);
        ROS_INFO("角度方差阀值 variance_min_yaw_lerance: %f", msg->variance_min_yaw_lerance);
        ROS_INFO("队列数量 optimizePoseWithTrilaterationPose_queue_size: %d", msg->optimizePoseWithTrilaterationPose_queue_size);


        if(!msg->firstTrilateration_with_yawFig){//firstTrilateration_with_yawFig_ 第一次定位成功就设置 false ,默认 true

            ROS_INFO("定位成功就设置 firstTrilateration_with_yawFig_  %s", "false");
        }else{
            ROS_INFO("默认 firstTrilateration_with_yawFig_  %s", "true");
        }
        if(msg->isTrilateration_with_yawFig){//第一次定位成功就设置 true,默认false
            ROS_INFO("定位成功就设置 isTrilateration_with_yawFig_  %s", "true");
        }else{
            ROS_INFO("默认 isTrilateration_with_yawFig_  %s", "false");
        }

        ROS_INFO("end=======================================================================================================================");
    }else if(msg->time_log_fig == 23){
        // 输出所有字段的数据
        // 输出所有字段的数据
        ROS_INFO("start========================================= time_log ==============================================================================");
        ROS_INFO("选择性能评估数据： Time Log Fig: %d - %s", msg->time_log_fig, getTimeLogFigDescription(msg->time_log_fig).c_str());
        ROS_INFO("Result Type: %s", msg->result_type.c_str());
        ROS_INFO("Class Name: %s", msg->className.c_str());
        ROS_INFO("Class Line: %d", msg->classLine);
        //if (msg->computeCurrentPose_score > msg->score_min_threshold && msg->computeCurrentPose_score <= msg->score_max_threshold) {
        if(msg->computeCurrentPoseFig){
            std::cout << YELLOW<< "Decoded Chinese Message: "<< decoded_chinese_message << ",得分: "<<msg->computeCurrentPose_score<< RESET<< std::endl;
        }else{
            std::cout << RED<< "Decoded Chinese Message: "<< decoded_chinese_message << ",得分: "<<msg->computeCurrentPose_score<< RESET<< std::endl;
        }

        //ROS_INFO("Decoded Chinese Message: %s", decoded_chinese_message.c_str());
        ROS_INFO("English Message: %s", msg->result_EN_message.c_str());


        ROS_INFO("Result Message: %s", msg->result_message.c_str());
        ROS_INFO("匹配上的反光柱数量： localization_hit_size，num_points: %d", msg->num_points);
        ROS_INFO("匹配上的数据ids： matchReflectorPaneIds，num_points: %s", msg->matchReflectorPaneIds.c_str());
        // 使用ROS_INFO打印Pose对象
        ROS_INFO("预估移动机器人位置 mapTbase_link-> Position: (%f, %f, %f)", msg->mapTbase_link.position.x, msg->mapTbase_link.position.y, msg->mapTbase_link.position.z);
        ROS_INFO("预估移动机器人姿态四元素 mapTbase_link-> Orientation: (%f, %f, %f, %f)", msg->mapTbase_link.orientation.x, msg->mapTbase_link.orientation.y, msg->mapTbase_link.orientation.z, msg->mapTbase_link.orientation.w);
        ROS_INFO("预估移动机器人姿态 mapTbase_link_yaw: %f", msg->mapTbase_link_yaw);

        //---------------------------------------------------------------------------------------------------------------------------------------
        ROS_INFO("得分 computeCurrentPose_score: %f", msg->computeCurrentPose_score);
        ROS_INFO("得分上限 score_max_threshold: %f", msg->score_max_threshold);
        ROS_INFO("得分下限 score_min_threshold: %f", msg->score_min_threshold);
        ROS_INFO("队列数量 optimizePoseWithTrilaterationPose_queue_size: %d", msg->optimizePoseWithTrilaterationPose_queue_size);
        //ROS_INFO("耗时： Execution Time: %s ms", msg->execution_time.c_str());
        std::cout <<GREEN << "耗时： Execution Time:: "<< msg->execution_time<<  RESET<< std::endl;



    }

    else  if(msg->time_log_fig!=12 && msg->time_log_fig!=17&& msg->time_log_fig!=18&&
       msg->time_log_fig!=19&& msg->time_log_fig!=22&& msg->time_log_fig!=23&& msg->time_log_fig!=24&& msg->time_log_fig!=25){
        // 输出所有字段的数据
        ROS_INFO("============================================================================================================");
        ROS_INFO("选择性能评估数据： Time Log Fig: %d - %s", msg->time_log_fig, getTimeLogFigDescription(msg->time_log_fig).c_str());
        ROS_INFO("Result Type: %s", msg->result_type.c_str());
        ROS_INFO("Class Name: %s", msg->className.c_str());
        ROS_INFO("Class Line: %d", msg->classLine);
        std::cout << YELLOW<< "Decoded Chinese Message: "<< decoded_chinese_message <<  RESET<< std::endl;
        //ROS_INFO("Decoded Chinese Message: %s", decoded_chinese_message.c_str());
        ROS_INFO("English Message: %s", msg->result_EN_message.c_str());


        ROS_INFO("Result Message: %s", msg->result_message.c_str());
        ROS_INFO("匹配上的反光柱数量： matchokReflectorPanelPointEnd_Unordered_map_size，num_points: %d", msg->num_points);
        ROS_INFO("匹配上的数据ids： matchReflectorPaneIds，num_points: %s", msg->matchReflectorPaneIds.c_str());
        // 使用ROS_INFO打印Pose对象
        ROS_INFO("计算得到的移动机器人位置 mapTbase_link-> Position: (%f, %f, %f)", msg->mapTbase_link.position.x, msg->mapTbase_link.position.y, msg->mapTbase_link.position.z);
        ROS_INFO("计算得到的移动机器人姿态四元素 mapTbase_link-> Orientation: (%f, %f, %f, %f)", msg->mapTbase_link.orientation.x, msg->mapTbase_link.orientation.y, msg->mapTbase_link.orientation.z, msg->mapTbase_link.orientation.w);
        ROS_INFO("算得到的移动机器人姿态 mapTbase_link_yaw: %f", msg->mapTbase_link_yaw);

        ROS_INFO("队列数量： matchokReflectorPanelPointSizes: %d",  msg->matchokReflectorPanelPointSize);
        //ROS_INFO("耗时： Execution Time: %s ms", msg->execution_time.c_str());
        std::cout <<GREEN << "耗时： Execution Time:: "<< msg->execution_time<<  RESET<< std::endl;

    }


    else{

        // 打印基础信息
        ROS_INFO("time_log_fig: %d", msg->time_log_fig);
        ROS_INFO("result_type: %s", msg->result_type.c_str());
        ROS_INFO("className: %s", msg->className.c_str());
        ROS_INFO("classLine: %d", msg->classLine);
        ROS_INFO("result_chinese_message: %s", msg->result_chinese_message.c_str());
        ROS_INFO("result_EN_message: %s", msg->result_EN_message.c_str());
        ROS_INFO("result_message: %s", msg->result_message.c_str());
        ROS_INFO("num_points: %d", msg->num_points);
        ROS_INFO("matchokReflectorPanelPointSize: %d", msg->matchokReflectorPanelPointSize);
        ROS_INFO("optimizePoseWithTrilaterationPose_queue_size: %d", msg->optimizePoseWithTrilaterationPose_queue_size);
        ROS_INFO("execution_time: %s", msg->execution_time.c_str());
        ROS_INFO("matchReflectorPaneIds: %s", msg->matchReflectorPaneIds.c_str());

        // 打印 Pose 信息
        ROS_INFO("mapTbase_link (position): x=%f, y=%f, z=%f", msg->mapTbase_link.position.x, msg->mapTbase_link.position.y, msg->mapTbase_link.position.z);
        ROS_INFO("mapTbase_link (orientation): x=%f, y=%f, z=%f, w=%f", msg->mapTbase_link.orientation.x, msg->mapTbase_link.orientation.y, msg->mapTbase_link.orientation.z, msg->mapTbase_link.orientation.w);
        ROS_INFO("mapTbase_link_yaw: %f", msg->mapTbase_link_yaw);

        // 打印计算与预测的位姿和偏航角
        ROS_INFO("calculate_mapTbase_link: x=%f, y=%f, z=%f", msg->calculate_mapTbase_link.position.x, msg->calculate_mapTbase_link.position.y, msg->calculate_mapTbase_link.position.z);
        ROS_INFO("predicted_mapTbase_link: x=%f, y=%f, z=%f", msg->predicted_mapTbase_link.position.x, msg->predicted_mapTbase_link.position.y, msg->predicted_mapTbase_link.position.z);
        ROS_INFO("calculate_mapTbase_link_yaw: %f", msg->calculate_mapTbase_link_yaw);
        ROS_INFO("predicted_mapTbase_link_yaw: %f", msg->predicted_mapTbase_link_yaw);

        // 打印里程计位姿和偏航角
        ROS_INFO("previous_odomTbase_link: x=%f, y=%f, z=%f", msg->previous_odomTbase_link.position.x, msg->previous_odomTbase_link.position.y, msg->previous_odomTbase_link.position.z);
        ROS_INFO("predicted_odomTbase_link: x=%f, y=%f, z=%f", msg->predicted_odomTbase_link.position.x, msg->predicted_odomTbase_link.position.y, msg->predicted_odomTbase_link.position.z);
        ROS_INFO("previous_odomTbase_link_yaw: %f", msg->previous_odomTbase_link_yaw);
        ROS_INFO("predicted_odomTbase_link_yaw: %f", msg->predicted_odomTbase_link_yaw);

        // 打印误差数据
        ROS_INFO("diff_variancePose_min: %f", msg->diff_variancePose_min);
        ROS_INFO("diff_varianceYaw_min: %f", msg->diff_varianceYaw_min);

        // 打印容差
        ROS_INFO("variance_min_pose_lerance: %f", msg->variance_min_pose_lerance);
        ROS_INFO("variance_min_yaw_lerance: %f", msg->variance_min_yaw_lerance);

        // 打印其他标志
        ROS_INFO("isTrilateration_with_yawFig: %s", msg->isTrilateration_with_yawFig ? "true" : "false");
        ROS_INFO("firstTrilateration_with_yawFig: %s", msg->firstTrilateration_with_yawFig ? "true" : "false");

        // 打印里程计与预测位姿的差异
        ROS_INFO("diff_previous_with_predicted_odomTbase_link_x: %f", msg->diff_previous_with_predicted_odomTbase_link_x);
        ROS_INFO("diff_previous_with_predicted_odomTbase_link_y: %f", msg->diff_previous_with_predicted_odomTbase_link_y);
        ROS_INFO("diff_previous_with_predicted_odomTbase_link_yaw: %f", msg->diff_previous_with_predicted_odomTbase_link_yaw);
        ROS_INFO("previous_with_predicted_distance: %f", msg->previous_with_predicted_distance);
        ROS_INFO("previous_with_predicted_distance_threshold: %f", msg->previous_with_predicted_distance_threshold);
        ROS_INFO("previous_with_predicted_distance_angle_threshold: %f", msg->previous_with_predicted_distance_angle_threshold);


    }


}
// rosrun   fanguangbantrilateration  fanguangbantrilateration_read_time_log_node
int main(int argc, char **argv)
{
  //  setlocale(LC_ALL, "");  //中文
    setlocale(LC_CTYPE,"zh_CN.utf8");
    ros::init(argc, argv, "fanguangbantrilateration_read_time_log_node");
    ros::NodeHandle n;

    ros::Subscriber sub = n.subscribe<fanguangbantrilateration::trilateration_read_time_log>("/trilateration_time_log", 1000, chatterCallback);

    ros::spin();

    return 0;
}

